#include "config.h" //通用配置头文件
#include "isr.h"

// 联合体：用于字节数组和整型之间的转换
union
{
	uint8_t u[4]; // 4 字节数组
	int32_t l;	  // 32 位有符号整型
} long_uchar;

union
{
	uint8_t u[2]; // 2 字节数组
	int16_t i;	  // 16 位有符号整型
} int_uchar;

nvmparam_t eeprom_data;

typedef struct
{
	uint16_t power;		   // ADC通道0数据
	uint16_t current_load; // ADC通道1数据
} adc_data_t;

volatile adc_data_t adc_data = {0}; // ADC数据结构体（初始化为0）
uint32_t dma_status;				// DMA 状态

uint16_t AD_PWR;

#define PWM_MODE 0 // PWM模式
#define CUR_MODE 1 // 力矩控制
#define SPD_MODE 2 // 速度控制
#define POS_MODE 3 // 位置控制
#define LAP_MODE 4 // 单圈绝对位置控制

#define MT_ONE_N 4 // 磁编码器校准次数

uint8_t MOTOR_MODE = POS_MODE;	   // 默认电机上电模式为位置控制模式
uint8_t MOTOR_MODE_OLD = POS_MODE; // 上一次控制模式

uint16_t CAN_ID;	   // CAN总线ID,默认=0xff
uint8_t CAN_RX_DAT[8]; // CAN缓冲
uint8_t CAN_TX_DAT[8]; // CAN缓冲

/////位置环采用绝对PID控制
#define POS_KP 0.07	   // 位置环比例系数0.08
#define POS_KI 0.0	   // 位置环积分系数
#define POS_KD 0.5	   // 位置环微分参数1.1
float SET_POS = 0;	   // 设定位置
float GET_POS = 0;	   // 实际位置
float ERR_POS = 0;	   // 位置偏差
float ERR_POS_old = 0; // 上一次位置偏差
float SET_SPD = 0;	   // 位置环输出的设定速度
float SET_SPD_Q = 0;   // 位置环输出的期望设定速度
float ERR_POS_I = 0.0; // 位置积分变量

/////速度环采用增量PID控制
float MAX_SPD = 420;   // 最大速度
float MAX_SPD_Q = 420; // 前置最大速度
#define SPD_KP 8.3	   // 速度环比例参数2.6
#define SPD_KI 0.33	   // 速度环积分参数0.42
#define SPD_KD 0.0	   // 速度环微分参数
float GET_SPD = 0;	   // 读取的实际输出速度
float ERR_SPD = 0;	   // 速度偏差
float ERR_SPD_old = 0; // 上一次速度偏差
float SPD_OUT = 0;	   // 速度环输出量
float SET_CUR = 0;	   // 速度环输出的设定电流 	实际值ma = 设定值*1.43

/////电流环采用增量PID控制
uint16_t I_offset = 1634; // 电流环中值，需矫正
#define MAX_PWM 9900	  // 最大占空比
int16_t MAX_CUR = 2000;	  // 限定最大电流，1530对应2.1A
#define CUR_KP 6.0		  // 电流环比例参数2.5
#define CUR_KI 8.8		  // 电流环积分参数6.8
#define CUR_KD 0.0		  // 电流环微分参数
int16_t GET_CUR = 0;	  // 电机电流
float ERR_CUR = 0;		  // 设定电流与实际电流的偏差
float ERR_CUR_old = 0;	  // 上一次的电流偏差
int16_t SET_PWM = 0;	  // 计算出的电流环输出PWM占空比

////电机回原点参数

bool HOME_DIR = 0;				// 电机回原点方向 0=逆时针，1=顺时针
bool HOME_MODE = 0;				// 0=碰撞回原点，1=单圈回原点
char HOME_SPD = 60;				// 回原点速度 30rpm , 最大120RPM
uint16_t HOME_MAX_TIME = 20000; // 回原点超时时间 10秒
uint16_t HOME_MAX_I = 400;		// 回原点碰撞电流 约500ma
uint16_t HOME_MAX_T = 5;		// 回原点碰撞检测次数
uint8_t HOME_MAX_L = 0;			// 最多回几圈
uint8_t HOME_I_T;				// 堵转时间计数
uint16_t HOME_T;				// 回零超时计数
bool HOME_OK = 0;				// 已回原点标志

uint16_t POS_input = 0;		// 读取的输出轴位置
uint16_t POS_input_old = 0; // 读取的输出轴上一次位置
uint8_t MT_one_flag = 0;	// 前几次采样标志
uint16_t LAP_OFFSET_POS;	// 磁铁绝对位置

uint16_t SPD_ACC = 0;

bool uart_flag = 0;
bool led_flag = 0;
bool CAN_RX_FLAG = 0;  // CAN接收标志
long int24;			   // 转换变量
uint8_t POS_START = 0; // 位置运行标志
uint8_t n, for_CRC;

float ACC_L = 0;	   // 加速减速距离
float POS_ERR_L = 0;   // 单次运动距离值
float POS_ERR_P = 0.0; // 当前运动完成比例
int16_t ACC_T = 0;	   // 计算加速度的间隔时间

static uint32_t last_timestamp = 0; // 上次时间戳
static uint32_t last_blink = 0;		// 上次闪烁时间
static uint32_t last_canBeat = 0;	// 上次心跳时间
/*************  本地函数声明    **************/
void GO_HOME(void);				 // 电机回原点
void PWM_OUT(int16_t pwm_value); // PWM输出函数
int main(void)
{
	// 延迟以便调试器连接（防止芯片被锁）
	for (volatile uint32_t i = 0; i < 1000000; i++)
		;

	System_init(); // 初始化一切

	if (NVM_Param_Init((uint32_t *)&eeprom_data)) // 如果读取eeprom失败 结果不为0
	{
		// 读取失败，使用默认值
		if (eeprom_data.can_id == 0)
		{
			eeprom_data.can_id = 0xFF; // 默认CAN ID
		}
		else if (eeprom_data.can_id == 0)
		{
			eeprom_data.can_id = 0xFF; // 默认CAN ID
		}
		CAN_ID = eeprom_data.can_id;
		LAP_OFFSET_POS = eeprom_data.position;  //读取磁铁绝对位置
	}

	set_systick_ms(1); // 设置SysTick中断周期为1ms（已在函数内部设置为最高优先级）
	delay_ticks(10);   // 延时10ms，等待系统稳定
	DL_GPIO_clearPins(LED_GRP_PORT, LED_GRP_LED_PIN_14_PIN);

	// 配置 DMA - 使用正确的 FIFO 地址
	DL_DMA_setSrcAddr(DMA, DMA_CH0_CHAN_ID, DL_ADC12_getFIFOAddress(ADC12_0_INST)); // 正确的 FIFO 地址（包含偏移）
	DL_DMA_setDestAddr(DMA, DMA_CH0_CHAN_ID, (uint32_t)&adc_data);					// 设置DMA目的地址为ADC数据结构体

	// 关键：先使能 DMA 通道，然后再启动 ADC
	DL_DMA_enableChannel(DMA, DMA_CH0_CHAN_ID); // 使能DMA通道

	// 启动 ADC 转换（这会触发 DMA）
	DL_ADC12_startConversion(ADC12_0_INST); // 启动ADC转换
	// 采样电流中值
	I_offset = 0; // 进行电流中值采样
	for (n = 0; n < 10; n++)
	{
		I_offset += adc_data.current_load;
		delay_ticks(1); // 延时1ms
	}
	I_offset /= 10;
	// 初始化MT6701
	MT_Init();
	// 设置中断优先级
	// 优先级 0 = 最高，3 = 最低（MSPM0 使用 2 位优先级）
	// SysTick 已设置为 0（最高），确保计时准确
	NVIC_SetPriority(TIMER_0_INST_INT_IRQN, 1); // 定时器中断优先级为 1
	NVIC_SetPriority(PWM_0_INST_INT_IRQN, 2);	// PWM 中断优先级为 2（较低）
	// while (1)  //调试IIC用
	// {
	// 	static float degree, radian;
	// 	MT_Read_Angle(MT_ADDR, MT_REG_ANGLE, MT_DAT_LEN, MT_dat); // 读取角度
	// 	degree = MT_Angle_To_Degree(MT_Get_Angle());
	// 	radian = MT_Angle_To_Radian(MT_Get_Angle());
	// 	// 读取的数据存放在 MT_dat 数组中
	// }
	NVIC_EnableIRQ(TIMER_0_INST_INT_IRQN); // 使能定时器中断
	NVIC_EnableIRQ(PWM_0_INST_INT_IRQN);   // 使能PWM中断
	__enable_irq();						   // 全局中断使能
	MT_one_flag = 0;
	MAX_CUR = 2000;
	DL_GPIO_setPins(LED_GRP_PORT, LED_GRP_LED_PIN_14_PIN);
	createTestFrame(); // 建立存活报文
	while (true)
	{
		if (msTicks - last_blink >= 500) // 每隔500ms执行一次
		{
			last_blink = msTicks;
			DL_GPIO_togglePins(LED_GRP_PORT, LED_GRP_LED_PIN_14_PIN); // 翻转LED状态
		}
		if (msTicks - last_canBeat >= 2000) // 维持边沿会抖动的2s一次心跳
		{
			last_canBeat = msTicks;
			can_tx_fifo_push(&i_am_alive_msg); // 发送CAN数据
		}
		if (POS_START && labs((int32_t)(SET_POS - GET_POS)) < 1024) // 角度已经控制到小于5°
		{
			//			printf("Ok\r\n",CAN_ID);
			switch (POS_START)
			{
			case 0x01:
			{
				CAN_TX_DAT[0] = 0xF4;					 // 返回相对位置到位
				CAN_TX_DAT[1] = 0x02;					 // 返回相对位置到位
				memcpy(txMsg.data, CAN_TX_DAT, 8);
				while (!can_tx_fifo_push(&txMsg)); // 发送CAN数据
			}
			break;
			case 0x02:
			{
				CAN_TX_DAT[0] = 0xF5;					 // 返回绝对位置到位
				CAN_TX_DAT[1] = 0x02;					 // 返回相对位置到位
				memcpy(txMsg.data, CAN_TX_DAT, 8);
				while (!can_tx_fifo_push(&txMsg)); // 发送CAN数据
			}
			break;
			}
			MAX_SPD = 420; // 到位后电机锁止
			POS_START = 0;
		}

		if (can_rx_fifo_pop(0, &rxMsg)) // 有新CAN数据
		{
			if (rxMsg.id == CAN_ID || rxMsg.id == 0) // 过滤CAN ID
			{
				for (int i = 0; i < 8; i++)
				{
					CAN_RX_DAT[i] = rxMsg.data[i];
				}
			}
			switch (CAN_RX_DAT[0]) // 判断功能码
			{
			case 0xf2: //-单圈绝对位置清零
			{
				LAP_OFFSET_POS = POS_input;

				NVM_Param_Write((uint32_t *)&eeprom_data); //存绝对位置
			}
			break;
			case 0xf3: //-单圈绝对位置控制
			{
				MOTOR_MODE = LAP_MODE;
				MOTOR_MODE_OLD = LAP_MODE;

				MOTOR_MODE_OLD = MOTOR_MODE;
				MAX_SPD = CAN_RX_DAT[1] * 256 + CAN_RX_DAT[2];
				if (CAN_RX_DAT[3] == 0)
					MAX_CUR = 1530; // 最大电流
				else
					MAX_CUR = CAN_RX_DAT[3] * 6; // 给电流值

				long_uchar.u[0] = CAN_RX_DAT[4]; // 使用联合体解包
				long_uchar.u[1] = CAN_RX_DAT[5];
				long_uchar.u[2] = CAN_RX_DAT[6];
				long_uchar.u[3] = CAN_RX_DAT[7];

				SET_POS = long_uchar.l; // 给到设定位置

				ERR_POS_I = 0.0;  // 位置偏差积分清0
				POS_START = 0x02; // 电机绝对位置模式开始运行
			}
			break;
			case 0xf4: //-相对位置控制
			{

				MOTOR_MODE = POS_MODE;
				if (MOTOR_MODE != MOTOR_MODE_OLD) // 如果从速度模式切换到位置模式
				{
					MT_one_flag = 0; // 重新校准位置
					SET_POS = 0;
				}
				while (MT_one_flag < MT_ONE_N)
					; // 等待校准完成
				MOTOR_MODE_OLD = MOTOR_MODE;
				MAX_SPD_Q = CAN_RX_DAT[1] * 256 + CAN_RX_DAT[2];
				SPD_ACC = CAN_RX_DAT[3]; // 加速度,RPM/(MIN*S)
				MAX_CUR = 1530;			 // 最大电流
				// if(CAN_RX_DAT[3]==0) 	MAX_CUR=1530; //最大电流
				// else MAX_CUR = CAN_RX_DAT[3]*6;  //根据ACC给电流值

				long_uchar.u[0] = CAN_RX_DAT[4]; // 使用联合体解包
				long_uchar.u[1] = CAN_RX_DAT[5];
				long_uchar.u[2] = CAN_RX_DAT[6];
				long_uchar.u[3] = CAN_RX_DAT[7];

				POS_ERR_L = long_uchar.l; // 获取单次运动的距离

				// 计算加速阶段的路程
				// printf("L=%d,",POS_ERR_L);

				if (SPD_ACC != 0) // 需要加减速控制
				{
					ACC_L = labs((int16_t)((((float)MAX_SPD_Q / (float)SPD_ACC) * (float)MAX_SPD_Q) * 16384 / (2 * 60))); // 计算路程   例如 16384 ACC=200,max=100   = 50*16384/120=6826.66 约0.4圈

					if ((2 * ACC_L) > labs((int16_t)POS_ERR_L))
						ACC_L = labs((int16_t)POS_ERR_L) / 2; // 没有匀速阶段,则只存在加减速阶段，加速一半，减速一半

					// 计算速度+1的间隔时间
					ACC_T = (int)1000 / SPD_ACC; // 计算每+1的时间，单位ms

					MAX_SPD = GET_SPD;

					// printf("Ok,M_Q=%d,ACC=%d,ACC_T=%d,ACC_L=%d;\r\n",MAX_SPD_Q,SPD_ACC,ACC_T,ACC_L);
				}

				SET_POS += POS_ERR_L; // 增量给到设定位置

				// printf(CAN_RX_DAT);
				// printf("SET_POS=%d\r\n",SET_POS);

				ERR_POS_I = 0.0;  // 位置偏差积分清0
				POS_START = 0x01; // 电机相对位置模式开始运行
			}
			break;
			case 0xf5: //-绝对位置控制
			{
				MOTOR_MODE = POS_MODE;
				if (MOTOR_MODE != MOTOR_MODE_OLD) // 如果从速度模式切换到位置模式
				{
					MT_one_flag = 0; // 重新校准位置
					SET_POS = 0;
				}
				MOTOR_MODE_OLD = MOTOR_MODE;
				MAX_SPD = CAN_RX_DAT[1] * 256 + CAN_RX_DAT[2];
				//						SPD_ACC = CAN_RX_DAT[3]; //加速度
				//						MAX_CUR=1530; //最大电流
				if (CAN_RX_DAT[3] == 0)
					MAX_CUR = 1530; // 最大电流
				else
					MAX_CUR = CAN_RX_DAT[3] * 6; // 给电流值

				long_uchar.u[0] = CAN_RX_DAT[4]; // 使用联合体解包
				long_uchar.u[1] = CAN_RX_DAT[5];
				long_uchar.u[2] = CAN_RX_DAT[6];
				long_uchar.u[3] = CAN_RX_DAT[7];

				SET_POS = long_uchar.l; // 给到设定位置

				ERR_POS_I = 0.0;  // 位置偏差积分清0
				POS_START = 0x02; // 电机绝对位置模式开始运行
			}
			break;
			case 0xf6: //-速度控制
			{
				MOTOR_MODE = SPD_MODE;
				MOTOR_MODE_OLD = MOTOR_MODE;

				int_uchar.u[0] = CAN_RX_DAT[1]; // 解包
				int_uchar.u[1] = CAN_RX_DAT[2];

				SET_SPD = int_uchar.i;

				if (CAN_RX_DAT[3] == 0)
					MAX_CUR = 1530; // 最大电流
				else
					MAX_CUR = CAN_RX_DAT[3] * 6; // 根据力矩给电流值
			}
			break;
			case 0xf7: //-PWM控制
			{
				MOTOR_MODE = PWM_MODE;
				MOTOR_MODE_OLD = PWM_MODE;

				int_uchar.u[0] = CAN_RX_DAT[1]; // 解包
				int_uchar.u[1] = CAN_RX_DAT[2];

				SET_PWM = int_uchar.i;

				if (CAN_RX_DAT[3] == 0)
					MAX_CUR = 1530; // 最大电流
				else
					MAX_CUR = CAN_RX_DAT[3] * 6; // 根据力矩给电流值

				CAN_TX_DAT[0] = 0x7f; // 返回相对位置到位
				int_uchar.i = GET_SPD;
				CAN_TX_DAT[1] = int_uchar.u[0];			 // 返回当前速度
				CAN_TX_DAT[2] = int_uchar.u[1];			 // 返回当前速度
				memcpy(txMsg.data, CAN_TX_DAT, 8);
				while (!can_tx_fifo_push(&txMsg)); // 发送CAN数据
			}
			break;
			case 0xf9: //-回零
			{
				HOME_MODE = CAN_RX_DAT[1];
				HOME_DIR = CAN_RX_DAT[2];
				HOME_SPD = CAN_RX_DAT[3];
				HOME_MAX_L = CAN_RX_DAT[4];
				GO_HOME();
				CAN_TX_DAT[0] = 0xF9;					 // 返回回零到位
				CAN_TX_DAT[1] = 0xF9;					 // 返回回零到位
				memcpy(txMsg.data, CAN_TX_DAT, 8);
				while (!can_tx_fifo_push(&txMsg)); // 发送CAN数据
			}
			break;
			}
			CAN_RX_FLAG = 0; // 标志复位
		}
	}
	return 0;
}

void GO_HOME(void) // 电机回原点
{
	int IN_CUR, IN_SPD;

	DL_GPIO_clearPins(LED_GRP_PORT, LED_GRP_LED_PIN_14_PIN);

	IN_CUR = MAX_CUR; // 记录原值
	IN_SPD = MAX_SPD;

	if (HOME_MODE == 0)	   // 多圈无限位回零
	{					   // 回零步骤：1.位置模式限制电流500ma反转0.1圈。2.速度模式正转30RPM。3.等待转速大于5转。3.开始检测转速小于5RPM且电流大于500ma且时长超过60ms为原点。
		if (HOME_DIR == 0) // 逆时针回原点
		{
			MOTOR_MODE = POS_MODE;
			MAX_SPD = 60; // 最大速度
			MAX_CUR = HOME_MAX_I;
			SET_POS = -1200;  // 反转约26度
			delay_ticks(200); // 延时
			MOTOR_MODE = SPD_MODE;
			SET_SPD = HOME_SPD; // 设定速度
			while (fabs(GET_SPD) < 5)
				; // 等待启动
		}
		else // 顺时针回原点
		{
			MOTOR_MODE = POS_MODE;
			MAX_SPD = 60; // 最大速度
			MAX_CUR = HOME_MAX_I;
			SET_POS = 1200;	  // 反转约26度
			delay_ticks(200); // 延时
			MOTOR_MODE = SPD_MODE;
			SET_SPD = -HOME_SPD; // 设定速度
			while (fabs(GET_SPD) < 5)
				; // 等待启动
		}
		// printf("START_GO_HOME\r\n");
		MAX_CUR = HOME_MAX_I + 200; // 设定最大电流比碰撞电流大250ma
		HOME_T = HOME_MAX_TIME;		// 设定最大时间
		HOME_I_T = HOME_MAX_T;
		while (HOME_T && HOME_I_T) // 等待碰撞
		{
			delay_ticks(1); // 延时
			if (GET_CUR > HOME_MAX_I && fabs(GET_SPD) < 5)
			{
				if (HOME_I_T > 0)
					HOME_I_T--;
			}
			else
				HOME_I_T = HOME_MAX_T;
		}
		MOTOR_MODE = POS_MODE;
		MOTOR_MODE_OLD = MOTOR_MODE;
		delay_ticks(20); // 延时
		// printf("START_MT=%d\r\n",(int)MT_one_flag);

		MT_one_flag = 0; // 重新校准位置
		while (!MT_one_flag)
			; // 等待校正完成

		// printf("END_MT=%d\r\n",(int)MT_one_flag);

		// delay_ticks(2000);  //延时
	}
	// printf("END_MT=%d\r\n",(int)MT_one_flag);
	POS_START = 0;
	MAX_CUR = IN_CUR; // 复原设定值
	MAX_SPD = IN_SPD;
	HOME_OK = 1; // 已回原点标志置位
	DL_GPIO_setPins(LED_GRP_PORT, LED_GRP_LED_PIN_14_PIN);
}

/**
 * @brief 电机 PWM 输出控制 (双通道互补输出)
 * @param pwm_value 带符号的 PWM 值 (-5333 ~ +5333)
 *        正值: CH0 输出 PWM, CH1 输出 0
 *        负值: CH0 输出 0, CH1 输出 PWM
 */
void PWM_OUT(int16_t pwm_value)
{
	uint32_t pwm_abs;

	// 限幅
	if (pwm_value > 5333)
		pwm_value = 5333;
	if (pwm_value < -5333)
		pwm_value = -5333;

	pwm_abs = (pwm_value > 0) ? pwm_value : -pwm_value;
	// H桥驱动一侧为1, 另一侧以低电平为占空比的PWM驱动
	// 反转则调转对侧为1, 另一侧以低电平为占空比的PWM驱动
	// 以实现占空高电平期间为1 - 1刹车状态
	if (pwm_value >= 0)
	{
		// 正转: CH0 输出 PWM, CH1 关闭
		DL_TimerG_setCaptureCompareValue(PWM_0_INST, pwm_abs, DL_TIMER_CC_0_INDEX);
		DL_TimerG_setCaptureCompareValue(PWM_0_INST, 0, DL_TIMER_CC_1_INDEX); // CH1 比较值0, low占空比0%, 等效一直为高电平
	}
	else
	{
		// 反转: CH0 关闭, CH1 输出 PWM
		DL_TimerG_setCaptureCompareValue(PWM_0_INST, 0, DL_TIMER_CC_0_INDEX); // CH0 比较值0, low占空比0%, 等效一直为高电平
		DL_TimerG_setCaptureCompareValue(PWM_0_INST, pwm_abs, DL_TIMER_CC_1_INDEX);
	}
}

void PWMB_isr(void) // 频率6KHZ
{
	if (MOTOR_MODE == CUR_MODE) //|| MOTOR_MODE == SPD_MODE || MOTOR_MODE == POS_MODE || MOTOR_MODE == LAP_MODE) //如果需要电流环
	{
		GET_CUR = adc_data.current_load - I_offset;						// 获取电机电流数值
		ERR_CUR = SET_CUR - GET_CUR;									// 电流偏差
		SET_PWM += (ERR_CUR - ERR_CUR_old) * CUR_KP + ERR_CUR * CUR_KI; // 增量型电流环PD控制
		ERR_CUR_old = ERR_CUR;											// 偏差迭代
	}
	else if (MOTOR_MODE == SPD_MODE || MOTOR_MODE == POS_MODE || MOTOR_MODE == LAP_MODE)
	{
		GET_CUR = adc_data.current_load - I_offset; // 获取电机电流数值
		ERR_CUR = SET_CUR - GET_CUR;				// 电流偏差
		if (ERR_CUR > MAX_CUR)
			ERR_CUR = MAX_CUR;
		if (ERR_CUR < -MAX_CUR)
			ERR_CUR = -MAX_CUR;
		if (fabs(ERR_CUR) <= MAX_CUR)
			SET_PWM = SET_CUR * 10000 / MAX_CUR; // 直接比例控制
		else
			SET_PWM += (ERR_CUR - ERR_CUR_old) * CUR_KP + ERR_CUR * CUR_KI; // 增量型电流环PD控制
		ERR_CUR_old = ERR_CUR;												// 偏差迭代
	}

	if (SET_PWM > MAX_PWM)
		SET_PWM = MAX_PWM; // 输出限幅
	if (SET_PWM < -MAX_PWM)
		SET_PWM = -MAX_PWM;
	PWM_OUT(SET_PWM); // PWM更新
}

void TM0_isr(void) // 1KHZ定时器
{
	static uint16_t T0_cnt = 0;
	if (++T0_cnt == 60000)
		T0_cnt = 0;

	if (HOME_T > 0)
		HOME_T--; // 回原点时间控制

	MT_Read_Angle(0x06, 0x03, 2, MT_dat);	 // 硬件IIC连续读数据
	POS_input = MT_dat[0] * 256 + MT_dat[1]; // 获得位置

	if (MT_one_flag < MT_ONE_N) // 校准编码器位置
	{
		POS_input_old = POS_input;
		GET_POS = 0;
		SET_POS = 0;
		MT_one_flag++;
	}

	// GET_SPD = ((int)(POS_input - POS_input_old)/4.0 - GET_SPD)*0.9 + GET_SPD *0.1; //获取速度

	GET_SPD = (int)(POS_input - POS_input_old);

	POS_input_old = POS_input; // 旧值迭代

	GET_POS += GET_SPD / 4; // 速度积分出位置

	// 速度环计算
	if (MOTOR_MODE == SPD_MODE || MOTOR_MODE == POS_MODE || MOTOR_MODE == LAP_MODE) // 如果需要速度环
	{
		ERR_SPD = SET_SPD - GET_SPD;
		SET_CUR += (ERR_SPD - ERR_SPD_old) * SPD_KP + ERR_SPD * SPD_KI; // 增量型PI控制速度环
		ERR_SPD_old = ERR_SPD;											// 偏差迭代

		if (SET_CUR > MAX_CUR)
			SET_CUR = MAX_CUR; // 输出限幅
		if (SET_CUR < -MAX_CUR)
			SET_CUR = -MAX_CUR;
	}

	if (T0_cnt % 2 == 0) // 500HZ位置环
	{
		// 位置环计算
		if (MOTOR_MODE == POS_MODE) // 如果电机模式是位置模式，则计算速度输出，否则不计算
		{
			ERR_POS = SET_POS - GET_POS;  // 计算位置偏差
			ERR_POS_I += ERR_POS * 0.001; // 积分位置偏差
			if (ERR_POS_I > 2400)
				ERR_POS_I = 2400; // 积分限幅 约60°
			if (ERR_POS_I < -2400)
				ERR_POS_I = -2400;

			SET_SPD = ERR_POS * POS_KP + ERR_POS_I * POS_KI + (ERR_POS - ERR_POS_old) * POS_KD; // 绝对型PID控制位置环
			if (SET_SPD > MAX_SPD)
				SET_SPD = MAX_SPD; // 输出限幅
			if (SET_SPD < -MAX_SPD)
				SET_SPD = -MAX_SPD;
			ERR_POS_old = ERR_POS; // 偏差迭代
		}
		else if (MOTOR_MODE == LAP_MODE) // 如果是单圈闭环模式
		{
			ERR_POS = (((float)SET_POS + 32768) - (float)(POS_input - LAP_OFFSET_POS)) / 4; // 计算位置偏差
			ERR_POS_I += ERR_POS * 0.001;													// 积分位置偏差
			if (ERR_POS_I > 2400)
				ERR_POS_I = 2400; // 积分限幅 约60°
			if (ERR_POS_I < -2400)
				ERR_POS_I = -2400;

			SET_SPD = ERR_POS * POS_KP + ERR_POS_I * POS_KI + (ERR_POS - ERR_POS_old) * POS_KD; // 绝对型PID控制位置环
			if (SET_SPD > MAX_SPD)
				SET_SPD = MAX_SPD; // 输出限幅
			if (SET_SPD < -MAX_SPD)
				SET_SPD = -MAX_SPD;
			ERR_POS_old = ERR_POS; // 偏差迭代
		}
	}

	if (T0_cnt % ACC_T == 0 && POS_START && SPD_ACC != 0) // 电机运行中进行加减速控制
	{
		if (labs(labs((long)POS_ERR_L) - labs((long)ERR_POS)) < ACC_L && MAX_SPD < MAX_SPD_Q)
			MAX_SPD += 1; // 在加速阶段
		if (labs((long)ERR_POS) < ACC_L && MAX_SPD > 25)
			MAX_SPD -= 1; // 在减速阶段
	}
	else if (SPD_ACC == 0)
		MAX_SPD = MAX_SPD_Q; // 不进行加减速

	if (T0_cnt % 40 == 0) // 40ms
	{
		// AD_PWR = ADC_get(ADC_P17)*1.62;  //获取电池电压
		uart_flag = 1;
	}

	if (T0_cnt % 500 == 0) // 500ms
	{
		led_flag = 1;
	}

	if (T0_cnt % 4000 == 0) // 5s
	{
		//		SET_CUR*=-1;
		//		SET_SPD*=-1;
		//		SET_POS*=-1;
		// uart_flag=1;
		// ID_EEPROM=1;
		//		if(HOME_OK)
		//		{
		//			if(SET_POS==-512)	SET_POS=-49152;
		//			else SET_POS=-512;
		//		}
		//		if(SET_POS==0)	SET_POS=-16384;
		//		else SET_POS=0;
	}
}